/*RF24 Basic Receiver*/
/*Adapted from J Coliz's superb RF24 library*/

/*
 This program is free software; you can redistribute it and/or
 modify it under the terms of the GNU General Public License
 version 2 as published by the Free Software Foundation.
 */

#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
#include <TimerOne.h>
#include <TimerThree.h>
#include <SoftwareSerial.h>

#define ARDUINO_RX 4//should connect to TX of the Serial MP3 Player module
#define ARDUINO_TX 5//connect to RX of the module
SoftwareSerial mySerial(ARDUINO_RX, ARDUINO_TX);

static int8_t Send_buf[8] = {0} ;

#define CMD_PLAY_W_INDEX 0X03
#define CMD_SET_VOLUME 0X06
#define CMD_SEL_DEV 0X09
#define DEV_TF 0X02
#define CMD_PLAY 0X0D
#define CMD_PAUSE 0X0E
#define CMD_SINGLE_CYCLE 0X19
#define SINGLE_CYCLE_ON 0X00
#define SINGLE_CYCLE_OFF 0X01
#define CMD_PLAY_W_VOL 0X22
#define CMD_STOP 0X16

// PWM Outpus
const int In_1_In_3 = 6;
const int In_2_In_4 = 7;

// Outputs para controle do sistema de direção
const int Dir_1 = 28;
const int Dir_2 = 29;

// Outputs da iluminação
const int luzBaixa = 30;
const int luzAlta = 31;
const int freio = 32;
const int alerta = 33;
const int luzRe = 34;

// Analog Inputs
const int steeringInput = A0;
const int ldrInput = A1;

// Flag de partida
boolean system_on = 0;

// Setpoint de direção
volatile int steeringSetpoint = 511;
//volatile int steeringSP = 511;
//volatile int desviox = 0;
//volatile int dirAux2 = 0;

// Set up nRF24L01 radio on SPI bus plus pins 9 & 10 
RF24 radio(48,53);

// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipe = 0xF0F0F0F0E1LL;

//container to hold the data

int rxData[3];
int audioSpeed = 0;

void setup(void)
{

  Serial.begin(57600);
  
  // set PWM pins as outputs:
  pinMode(In_1_In_3, OUTPUT);
  pinMode(In_2_In_4, OUTPUT);
  
  // Set Light pins as outputs:
  for (int i = luzBaixa; i <= luzRe; i++) { 
    pinMode(i, OUTPUT); 
  }
  
  // Configure PWM frequency on pins 6, 7 and 8
  TCCR4B = (TCCR4B & 0xF8) | 0x02;
  
  printf_begin();
  mySerial.begin(9600);
  delay(1000);
  sendCommand(CMD_SEL_DEV, DEV_TF);//select the TF card
  
  SetupReceiver();
  
  Timer1.initialize(500000);         // initialize timer1, and set a 1/2 second period
  Timer3.initialize(50000);
  Timer3.attachInterrupt(PositionControl);
}

void sendCommand(int8_t command, int16_t dat)
{
  delay(20);
  Send_buf[0] = 0x7e; //starting byte
  Send_buf[1] = 0xff; //version
  Send_buf[2] = 0x06; //the number of bytes of the command without starting byte and ending byte
  Send_buf[3] = command; //
  Send_buf[4] = 0x00;//0x00 = no feedback, 0x01 = feedback
  Send_buf[5] = (int8_t)(dat >> 8);//datah
  Send_buf[6] = (int8_t)(dat); //datal
  Send_buf[7] = 0xef; //ending byte
  for(uint8_t i=0; i<8; i++)//
  {
    mySerial.write(Send_buf[i]) ;
  }
}

void PiscaAlerta()
{
  digitalWrite(alerta, digitalRead(alerta) ^ 1);
}

void PositionControl()
{
  int dirAux;
  int desvio;
  int setPoint;
  int posRead;
  
  dirAux = steeringSetpoint - 511;
//  dirAux2 = dirAux;
  desvio = dirAux*0.126;
//  desviox = desvio;
  setPoint = 511 + desvio;
//  steeringSP = setPoint;
  
  posRead = analogRead(steeringInput);
  
  if(posRead < (setPoint - 0.01*setPoint))
  {
    digitalWrite(Dir_1, HIGH);
    digitalWrite(Dir_2, LOW);
  }
  else
  {
    if(posRead > (setPoint + 0.01*setPoint))
    {
      digitalWrite(Dir_1, LOW);
      digitalWrite(Dir_2, HIGH);
    }
    else
    {
      digitalWrite(Dir_1, LOW);
      digitalWrite(Dir_2, LOW);
    }
  }
}

void SetupReceiver()
{
   // Setup and configure rf radio
  radio.begin();
  radio.setDataRate(RF24_2MBPS);//data rate
  radio.setPALevel(RF24_PA_MAX); //txn power output
  radio.setRetries(15,15);
  radio.setPayloadSize(32);
  radio.setChannel(100);//set the channel to use
  radio.openReadingPipe(1,pipe);
  radio.startListening();
  radio.printDetails();
}

// Recebimento de mensagens do rádio
boolean NewIncomingData()
{
  boolean done = false;
    
  if(radio.available())
  {
    while(!done)
    {
      done = radio.read(rxData,sizeof(rxData));
  
    }
  } 
  return done;
}

// Controle de velocidade e direção
void  SpeedControl(int speedVal)
{
  byte dir = 0; // Direção default FRENTE
  int desvio;
  int setPoint;
  
  speedVal = speedVal - 127;
  
  if(speedVal != 0)
  {
    if(speedVal < 0)
    {
      dir = 1; // Define direção para RÉ
      if(audioSpeed != 9)
      {
        sendCommand(CMD_PLAY_W_VOL, 0X1F05);//play the first song with volume 30 class
        audioSpeed = 9;
      }
    }
    desvio = abs(speedVal)*155/128;
    
    setPoint = 100 + desvio;
    Serial.print("SetPoint:  ");
    Serial.println(setPoint);
    // Aciona controladores do PWM
    PWMControl(setPoint, dir);
    if(dir == 0)
    {
      if((audioSpeed != 1) && (setPoint < 140))
      {
        sendCommand(CMD_PLAY_W_VOL, 0X1F06);//play the first song with volume 30 class
        audioSpeed = 1;
        Serial.println("SPEED 1");
      }
      if((audioSpeed != 2) && (setPoint >= 140) && (setPoint < 180))
      {
        sendCommand(CMD_PLAY_W_VOL, 0X1F07);//play the first song with volume 30 class
        audioSpeed = 2;
                Serial.println("SPEED 2");
      }
      if((audioSpeed != 3) && (setPoint >= 180) && (setPoint < 220))
      {
        sendCommand(CMD_PLAY_W_VOL, 0X1F08);//play the first song with volume 30 class
        audioSpeed = 3;
                Serial.println("SPEED 3");
      }
      if((audioSpeed != 4) && (setPoint > 220))
      {
        sendCommand(CMD_PLAY_W_VOL, 0X1F09);//play the first song with volume 30 class
        audioSpeed = 4;
        Serial.println("SPEED 4");
      }
    }
  }
  else
  {
    PWMControl(0,0);
    if(audioSpeed != 0)
    {
      sendCommand(CMD_PLAY_W_VOL, 0X1F03);//play the first song with volume 30 class
      audioSpeed = 0;
      Serial.println("SPEED 0");
    }
  }
}

// Controle do PWM
void PWMControl(int sp, byte dir)
{
  switch(dir)
  {
    case 0:
      analogWrite(In_1_In_3, sp);
      analogWrite(In_2_In_4, 0);
      break;
    case 1:
      analogWrite(In_2_In_4, sp);
      analogWrite(In_1_In_3, 0);
      break;
  }
}

// Controle da iluminação
void LightControl(int lightCommand)
{
  int ldr = analogRead(ldrInput);
  Serial.print("LDR value:");
  Serial.println(ldr);
  
  boolean farol_on = 0;
  boolean alerta_on = 0;
  
  // Controle da luz baixa
  if((lightCommand & 0x2) || ((lightCommand & 0x4) && (ldr <= 500)))
  {
    digitalWrite(luzBaixa, HIGH);
    farol_on = 1;
  }
  else
  {
    digitalWrite(luzBaixa, LOW);
    farol_on = 0;
  }
  
  // Controle da luz alta
  if((lightCommand & 0x8) && farol_on)
  {
    digitalWrite(luzAlta, HIGH);
  }
  else
  {
    digitalWrite(luzAlta, LOW);
  }
  
  // Controle do alerta
  if(lightCommand & 0x10)
  {
    if(!alerta_on)
    {
      Timer1.attachInterrupt(PiscaAlerta);  // attaches callback() as a timer overflow interrupt
      alerta_on = 1;
      Serial.println("Alerta ativado!");
    }
  }
  else
  {
    Timer1.detachInterrupt();
    alerta_on = 0;
    digitalWrite(alerta, LOW);
  }
}

// Loop principal
void loop(void)
{

    // if there is new data available...
    
    if(NewIncomingData())
    {
        //do something with the incoming data packet 
        
//        Serial.print("Rx[0]:");
//        Serial.print(rxData[0]);
//        Serial.print("   Steering:");
//        Serial.print(rxData[1]);
//        Serial.print("   Speed:");
//        Serial.print(rxData[2]);
//        Serial.print("   Speed maped:");
//        Serial.println(map(rxData[2], 0, 1023, 0, 255));
//        
//        Serial.print("DirAux:");
//        Serial.println(dirAux2);
//        Serial.print("Desvio:");
//        Serial.println(desviox);
//        Serial.print("Steering SP:");
//        Serial.println(steeringSP);
//        
//        Serial.print("Steering REAL:");
//        Serial.println(analogRead(steeringInput));
        
        if(rxData[0] & 0x1) // Systema ligado
        {
          if(!system_on)
          {
            digitalWrite(luzBaixa, HIGH);
            digitalWrite(freio, HIGH);
            digitalWrite(luzRe, HIGH);
            digitalWrite(alerta, HIGH);
            sendCommand(CMD_PLAY_W_VOL, 0X1F01);//play the first song with volume 30 class
            delay(2000);
            digitalWrite(luzBaixa, LOW);
            digitalWrite(freio, LOW);
            digitalWrite(luzRe, LOW);
            digitalWrite(alerta, LOW);
            system_on = 1; // Sistema inicializado
            sendCommand(CMD_PLAY_W_VOL, 0X1F02);//play the first song with volume 30 class
          }
          
          // Controla luz de freito e luz de ré
          if(rxData[2] == 511)
            digitalWrite(freio, HIGH);
          else
            digitalWrite(freio, LOW);
          
          if(rxData[2] < 511)
            digitalWrite(luzRe, HIGH);
          else
            digitalWrite(luzRe, LOW);
          
          // Controle da direção
          steeringSetpoint = rxData[1];
          // Controle da valocidade
          SpeedControl(map(rxData[2], 0, 1023, 0, 255));
          // Controle da iluminação
          LightControl(rxData[0]);
        }
        else
        {
          
          
          // Turn OFF all the lights
          for (int pin = luzBaixa; pin <= luzRe; pin++) { 
            digitalWrite(pin, LOW); 
          }
          
          // Seta velocidade para zero
          SpeedControl(127);
          
          // Seta direção para posição original
          steeringSetpoint = 511;
          if(system_on == 1)
          {
            sendCommand(CMD_PLAY_W_VOL, 0X1F04);//play the first song with volume 30 class
            delay(3000);
            sendCommand(CMD_STOP, 0X0);//play the first song with volume 30 class
          }
          
          system_on = 0; // Sistema desligado
        }
    }
    else
    {
      Serial.println("Nothing to read");
    }
    // Delay
    delay(30);
}
